#include "canopen_can.h"
#include "can.h"       
#include "canfestival.h" 

static CO_Data *co_data = NULL;


unsigned char CAN1_Init(CO_Data *d)
{
    // 过滤器配置
    CAN_FilterTypeDef filter_config = {0};
    
    co_data = d;

    filter_config.FilterBank = 0;
    filter_config.FilterMode = CAN_FILTERMODE_IDMASK;
    filter_config.FilterScale = CAN_FILTERSCALE_32BIT;
    filter_config.FilterIdHigh = 0x0000;
    filter_config.FilterIdLow = 0x0000;
    filter_config.FilterMaskIdHigh = 0x0000;
    filter_config.FilterMaskIdLow = 0x0000;
    filter_config.FilterFIFOAssignment = CAN_RX_FIFO0;
    filter_config.FilterActivation = CAN_FILTER_ENABLE;
    filter_config.SlaveStartFilterBank = 14; 

    if (HAL_CAN_ConfigFilter(&hcan1, &filter_config) != HAL_OK) {
        return 0;
    }

    if (HAL_CAN_Start(&hcan1) != HAL_OK) {
        return 0;
    }
    
    HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
    return 1;
}


unsigned char canSend(CAN_PORT notused, Message *m)
{
    CAN_TxHeaderTypeDef tx_header = {0};
    uint32_t mailbox;
    uint8_t tx_data[8] = {0};
    uint32_t start_tick = HAL_GetTick();

    
    tx_header.StdId = m->cob_id;
    tx_header.IDE = CAN_ID_STD;
    tx_header.RTR = m->rtr ? CAN_RTR_REMOTE : CAN_RTR_DATA;
    tx_header.DLC = m->len;
    tx_header.TransmitGlobalTime = DISABLE;


    for (int i = 0; i < m->len; i++) {
        tx_data[i] = m->data[i];
    }

    // 若无空闲邮箱则等待一段时间
    while(HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0) {
        if(HAL_GetTick() - start_tick > 2) { 
            return 0;
        }
    }
    
  
    if (HAL_CAN_AddTxMessage(&hcan1, &tx_header, tx_data, &mailbox) != HAL_OK) {
        return 0; 
    }
    
    return 1; 
}


void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    int i;
    CAN_RxHeaderTypeDef RxHeader;  
    uint8_t RxData[8] = {0};       
    Message rxm = {0};             
    
   
    if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, RxData) != HAL_OK)
    {
        
        Error_Handler();
    }
    
   
    if (RxHeader.IDE == CAN_ID_EXT)
        return;
    
    
    rxm.cob_id = RxHeader.StdId;
    if (RxHeader.RTR == CAN_RTR_REMOTE)
        rxm.rtr = 1;
    rxm.len = RxHeader.DLC;
    
    for (i = 0; i < rxm.len; i++)
        rxm.data[i] = RxData[i];
    
  
    if ((rxm.cob_id >> 7) == 0xB)  
    {
        int16_t test = 0;
        switch (rxm.cob_id)
        {
            case 0x582:
                test = rxm.data[4] | (rxm.data[5] << 8);
                printf("test=%d,\r\n", test);  
                break;
            default:
                break;
        }
    }
    else
    {
        canDispatch(co_data, &rxm);  
    }
    
    
    HAL_CAN_ActivateNotification(hcan, CAN_IT_RX_FIFO0_MSG_PENDING);
}
